#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：henan_sumo
@File ：simu_base.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/6/9 13:57
'''
import json
import math
import time
import threading
from collections import defaultdict
from multiprocessing import Process, Queue

import sumolib
import traci.constants as tc
from kafka import KafkaProducer, KafkaConsumer
from loguru import logger

from config.config40 import GLOBAL_POLYGON_CENTER, MAX_SPEED, RADAR_DATA_LIST, USE_LIBSUMO, USE_GUI, KAFKA_E1_TOPIC, \
    KAFKA_HOST, SHOW_SIMU, KAFKA_E1_TOPIC_SIMU
from utils import sumo_utils
from utils.VehiclePerceptionPredictor import VehiclePerceptionPredictor
from utils.sumo_utils import calculate_distance
from utils.tools import find_vehicles_by_duration_optimized

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui):
        """
           仿真初始化
           :param sumo_net_file:  仿真路网文件路径
           :param sumo_cfg_file:  仿真配置文件路径
           :param gui: 是否开启gui
        """
        # ---------------------------- 路网相关配置 -----------------------------
        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        self.sumo_net_instance = None
        # 基站路由配置
        self.route_basestation = dict()
        # ############################### 轨迹数据 ##############################
        # 用于接收基站感知轨迹数据
        self.trace_queue = Queue(maxsize=2000)
        # 所有在网仿真车辆
        self.global_vehicles = None
        # 所有门架数据
        self.gantry_info_data = defaultdict(lambda: defaultdict(dict))
        # 发车信息表
        self.sn_send_car_info_dict = defaultdict(dict)
        # 实时仿真推演数据
        self.predictions = None
        # 仿真匹配集合
        self.sim_match_sn = defaultdict(dict)

        # ################################ 其他配置 #############################
        # 创建仿真推演预测对象
        self.predictor = VehiclePerceptionPredictor(min_duration=0.5, frame_interval=0.1, predict_frames=20)
        # kafka 数据监听
        self.kafka_e1_topic = KAFKA_E1_TOPIC
        self.kafka_host = KAFKA_HOST
        self.kafka_e1_sim = KAFKA_E1_TOPIC_SIMU
        # self.son_process = threading.(target=self.read_traffic_channel,
        #                            args=(self.kafka_e1_topic,), daemon=True)
        # self.son_process.start()
        threading.Thread(target=self.read_traffic_channel, args=(self.kafka_e1_topic,)).start()
        # 数据发送线程
        threading.Thread(target=self.send_car_info, args=(self.kafka_e1_topic,)).start()
        # ################################ 初始化配置 #############################
        self.init_sumo()

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度（150km/h）
        for edge_id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(edge_id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, tc.VAR_ACCEL])

        # 将雷达、卡口相机的经纬度 映射到路网上
        for sn, sn_info in RADAR_DATA_LIST.items():
            sn = str(sn)
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            # 初始化路由
            edge_id, pos, lane = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
            if sn_info['sn'] not in self.route_basestation.keys():
                traci.route.add(sn, [str(edge_id), '-87'])
                self.route_basestation[sn] = {"edge_id": edge_id, "pos": pos, "lane": lane, "route_id": sn}
            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, text=sn, radius=50)
            else:
                self.create_circle_polygon(sn, center_x, center_y, text=sn, radius=50, color=(255, 0, 0, 255))

    def main_loop(self):
        # 车辆记录
        all_set = set()
        while True:
            start_time = time.time()
            # 获取当前仿真在网车辆
            self.global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            # 车辆发车控制
            self.control_send_car()
            # 车辆行为控制
            self.control_lidar_car()
            # 删车控制
            self.control_delete_car()

            traci.simulationStep()
            end_time = time.time()
            all_set.update(set(self.global_vehicles.keys()))
            logger.info(f"仿真数量总数{len(all_set)}")
            if start_time and (end_time - start_time) < 0.1:
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    pass

    def control_send_car(self):
        start_time = int(time.time() * 1000)
        # 判断车辆信息，是否需要进行仿真推演
        gantry_info_data = self.gantry_info_data.copy()
        predictions = self.predictor.process_vehicle_data(gantry_info_data)
        self.predictions = predictions

        prediction_time = int(time.time() * 1000)
        print('推演总耗时：', prediction_time - start_time)
        # 判断车辆是否消失
        continuous_vehicles, disappeared_vehicles, delete_vehicles = find_vehicles_by_duration_optimized(
            gantry_info_data, 100, 1000, 50000)
        # 获取消失的车辆
        for sn, disappeared_vehicle_list in disappeared_vehicles.items():
            for disappeared_vehicle in disappeared_vehicle_list:
                dis_id = disappeared_vehicle[0]
                # 获取预测帧该车辆的信息
                if sn in predictions.keys():
                    if dis_id in predictions[sn].keys():
                        if predictions[sn].get(dis_id):
                            prediction = predictions[sn].get(dis_id)
                            prediction_keys = prediction.keys()
                            prediction_keys = sorted(prediction_keys)
                            match_key = prediction_keys[9]
                            # 获取预测帧该车辆的信息
                            prediction_info = prediction.get(match_key)
                            # print("预测信息为：", prediction_info)
                            # 添加车辆信息
                            car_id = str(prediction_info['sn']) + "_" + str(prediction_info['id'])
                            if car_id not in self.sn_send_car_info_dict[sn].keys():
                                self.add_gantry_info(prediction_info, sn)
        # 判断门架数据是否已满足断面数据条件
        print('发车总耗时：', int(time.time() * 1000) - prediction_time)

    def control_lidar_car(self):
        # 限定雷达存在2个断面 匹配断面、消失断面
        # 获取雷达基站实时上报数据
        gantry_info_data = self.gantry_info_data.copy()
        sn_send_car_info_dict = self.sn_send_car_info_dict.copy()
        global_vehicles = self.global_vehicles.copy()
        for sn, sn_info in gantry_info_data.items():
            for obj_id, obj_info in list(sn_info.items()):
                # 判断时间对应数据是否只有1帧，如果只有1帧，则认定为新增感知车辆
                if len(obj_info.keys()) == 1:
                    # 获取仿真区间内车辆
                    if sn in RADAR_DATA_LIST.keys():
                        if RADAR_DATA_LIST[sn]['Upstream'] is not None:
                            # 获取上游门架id
                            sn_up = RADAR_DATA_LIST[sn]['Upstream']['sn']
                            if sn_up in sn_send_car_info_dict.keys():
                                car_list = sn_send_car_info_dict[sn_up]
                                # 获取距离雷达点位最近的车辆
                                min_car_id = None
                                min_dis = None
                                min_veh_info = None
                                min_dis_next_sn = None
                                for car_id in car_list.keys():
                                    # 判断车辆是否已匹配
                                    if car_id in self.sim_match_sn[sn].keys():
                                        continue
                                    veh_info = dict(global_vehicles).get(car_id)
                                    # 判断车辆是否在网
                                    if veh_info is None:
                                        continue
                                    # 寻找未匹配的车辆，距离匹配断面小于200m的车辆
                                    pos = veh_info[tc.VAR_POSITION]
                                    longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
                                    dis_next_sn = calculate_distance(longitude, latitude,
                                                                     RADAR_DATA_LIST[sn]['lon'],
                                                                     RADAR_DATA_LIST[sn]['lat'])
                                    if dis_next_sn < 200:
                                        if min_dis is None:
                                            min_dis = dis_next_sn
                                            min_car_id = car_id
                                            min_veh_info = veh_info
                                            min_dis_next_sn = dis_next_sn
                                        else:
                                            if dis_next_sn < min_dis:
                                                min_dis = dis_next_sn
                                                min_car_id = car_id
                                                min_veh_info = veh_info
                                                min_dis_next_sn = dis_next_sn
                                # 找到最佳匹配车辆
                                if min_car_id:
                                    print("min_car_id: ", min_car_id, min_dis, min_dis_next_sn)
                                    # 判断车辆加减速
                                    all_inner_dicts = list(obj_info.values())
                                    # 取第一个内部字典
                                    obj_info_detail = all_inner_dicts[0]
                                    print("obj_info_detail:", obj_info_detail)
                                    print("匹配车辆信息为：", str(obj_info_detail["id"]), min_car_id)
                                    self.sim_match_sn[sn][min_car_id] = str(obj_info_detail["id"])
                                    dis_next_sn = calculate_distance(obj_info_detail['lon'], obj_info_detail['lat'],
                                                                     RADAR_DATA_LIST[sn]['lon'],
                                                                     RADAR_DATA_LIST[sn]['lat'])
                                    speed = int(min_veh_info[tc.VAR_SPEED])
                                    if dis_next_sn - min_dis_next_sn < 5:
                                        # 拼接成功 移除仿真车辆
                                        traci.vehicle.remove(min_car_id)
                                        print("拼接成功, 移除仿真车辆：", min_car_id)
                                        if min_car_id in self.sn_send_car_info_dict[sn].keys():
                                            self.sn_send_car_info_dict[sn].pop(min_car_id)
                                    elif dis_next_sn < min_dis_next_sn:
                                        if min_car_id in traci.vehicle.getIDList():
                                            # 仿真车辆做减速
                                            traci.vehicle.setSpeed(str(min_car_id), speed * 0.8)
                                        else:
                                            print("仿真车辆做减速", traci.vehicle.getIDList())
                                    else:
                                        if min_car_id in traci.vehicle.getIDList():
                                            # 仿真车辆做加速
                                            traci.vehicle.setSpeed(str(min_car_id), speed * 1.2)
                                        else:
                                            print("仿真车辆做加速", traci.vehicle.getIDList())

        # 对于已经匹配的车辆实时计算感知车辆最新一帧与仿真车最新一帧的距离
        sim_match_sn = self.sim_match_sn.copy()
        # print('实时匹配车辆信息为：', sim_match_sn)
        # 实时计算仿真车辆与感知车辆的距离
        for sn_id, car_id in sim_match_sn.items():
            if "_" not in sn_id:
                continue
            sn_id_list = sn_id.split("_")
            print(sn_id, car_id)
            sn_id = sn_id_list[0]
            sn_car_id = sn_id_list[1]
            if car_id not in dict(global_vehicles).keys():
                continue
            veh_info = dict(global_vehicles).get(str(car_id))
            # 从门架获取对应数据
            if sn_id in gantry_info_data.keys():
                if car_id in gantry_info_data[sn_id].keys():
                    ts_list = sorted(gantry_info_data[sn_id][sn_car_id].keys())
                    last_ts = ts_list[-1]
                    car_info = gantry_info_data[sn_id][sn_car_id][last_ts]
                    lon = car_info['lon']
                    lat = car_info['lat']
                    pos = veh_info[tc.VAR_POSITION]
                    longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
                    dis_next_sn = calculate_distance(lon, lat, longitude, latitude)
                    if dis_next_sn < 50:
                        traci.vehicle.remove(car_id)
                        self.sn_send_car_info_dict[sn_id].pop(car_id)
                        print("距离", sn_id, "小于50", "强制删除车辆：", car_id)

    def control_delete_car(self):
        global_vehicles = self.global_vehicles.copy()
        for vehicle_id, veh_info in dict(global_vehicles).items():
            pos = veh_info[tc.VAR_POSITION]
            longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
            sn_id_list = vehicle_id.split("_")
            sn_id = sn_id_list[0]
            if sn_id in RADAR_DATA_LIST.keys():
                if RADAR_DATA_LIST[sn_id]['Downstream'] is not None:
                    sn_down = RADAR_DATA_LIST[sn_id]['Downstream']['sn']
                    dis_next_sn = calculate_distance(longitude, latitude, RADAR_DATA_LIST[sn_down]['lon'],
                                                     RADAR_DATA_LIST[sn_down]['lat'])
                    if dis_next_sn <= 50:
                        traci.vehicle.remove(vehicle_id)
                        print("距离下游门架", sn_down, "距离小于50", "强制删除车辆：", vehicle_id)
                        self.sn_send_car_info_dict[sn_id].pop(vehicle_id)
                else:
                    dis_next_sn = calculate_distance(longitude, latitude,
                                                     RADAR_DATA_LIST[sn_id]['lon'],
                                                     RADAR_DATA_LIST[sn_id]['lat'])
                    if dis_next_sn >= 200:
                        traci.vehicle.remove(vehicle_id)
                        print("距离上游门架", sn_id, "距离大于200", "删除车辆：", vehicle_id)
                        self.sn_send_car_info_dict[sn_id].pop(vehicle_id)

    def add_gantry_info(self, car_info, sn):
        car_id = str(car_info['sn']) + "_" + str(car_info['id'])
        set_speed = car_info['speed']
        edge_id, pos, lane = traci.simulation.convertRoad(car_info['lon'], car_info['lat'], True)
        # print("车辆发车车型",car_info['type'])
        traci.vehicle.add(
            vehID=str(car_id),
            routeID=self.route_basestation[sn]['route_id'],
            departPos=pos,
            typeID=int(car_info['type']), depart='now',
            departSpeed=str(set_speed))
        print("添加车辆：", car_id)
        traci.vehicle.setSpeed(car_id, set_speed)
        self.sn_send_car_info_dict[sn][car_id] = car_info['plate']

    def create_circle_polygon(self, polygon_id, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True,
                              text=None, text_color=(0, 0, 0, 255)):
        """
        在指定位置创建一个近似圆形的多边形，并可选择添加中心文本。

        :param polygon_id: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        :param text: 要在圆心显示的文本，None 表示不显示文本
        :param text_color: 文本颜色 (R,G,B,A)，默认黑色不透明
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{polygon_id}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

        # # 添加文本（如果指定）
        # if text:
        #     text_id = f"text_{polygon_id}"
        #     traci.poi.add(text_id, center_x, center_y,
        #                   color=text_color, layer=1)
        #     traci.poi.setText(text_id, text)  # 正确设置文本的方法
